#pragma once
#include <array>
#include <algorithm>
template <class T, class Operation>
void transform(T& a, Operation op) {
    std::transform(a.begin(), a.end(), a.begin(), op);
}

template <class T, class Operation> 
void transform(T& a, const T& b, Operation op) {
    std::transform(a.begin(), a.end(), b.begin(), a.begin(), op);
}

template <class T, int R, int C>
class matrix {
    public:
    std::array<T,R*C> data;
    matrix() { std::fill(data.begin(), data.end(), 0); }
    matrix(const std::initializer_list<T>& list) { 
        std::copy(list.begin(), list.begin() + R*C, data.begin());
    }
    template <int N>
    matrix(const matrix<T,N,1>& other) {
        int end = (R < N) ? R : N;
        for (int i = 0; i < end; ++i) data[i] = other.data[i]; //copy the common parts
        for (int i = N; i < R; ++i) data[i] = 1; //fill the rest with 1, this propably should be documented
    }
    T& operator()(int c, int r) { return data[c*C+r]; }
    const T& operator()(int c, int r) const { return data[c*C+r]; }
    T& operator[](int i) { return data[i]; }
    const T& operator[](int i) const { return data[i]; }
    
    matrix<T,R,C>& operator+=(const matrix<T,R,C>& b) {
        transform(data, b.data, [](T a, T b) { return a + b; });
        return *this;
    }
    
    matrix<T,R,C>& operator-=(const matrix<T,R,C>& b) {
        transform(data, b.data, [](T a, T b) { return a - b; });
        return *this;
    }

    matrix<T,R,C>& operator*=(const T& b) {
        transform(data, [&](T a) { return a * b; });
        return *this;
    }

    matrix<T,R,C>& operator/=(const T& b) {
        transform(data, [&](T a) { return a / b; });
        return *this;
    }

    T norm() {
        static_assert( C == 1, "This method is only for vectors" );
        T l = 0;
        for (int i = 0; i < R; ++i) l += data[i] * data[i];
        return sqrt(l);
    }

    matrix<T,R,C>& normalize() {
        static_assert( C == 1, "This method is only for vectors");
        T n = 0.0f;
        n = 1.0f / norm();
        for (int i = 0; i < R; ++i) data[i] *= n;
        return *this;
    }

    matrix<T,R,C> normalized() const {
        static_assert( C == 1, "This method is only for vectors");
        matrix<T,R,C> v = *this;
        return v.normalize();
    }

    matrix<T,R,C> cross(const matrix<T,R,C>& b) {
        static_assert( R == 3 and C == 1, "This method is only for 3-dimensional vectors");
        matrix<T,R,C>& a = *this;
        matrix<T,R,C> tmp;
        tmp[0] = a[1] * b[2] - a[2] * b[1];
        tmp[1] = a[2] * b[0] - a[0] * b[2];
        tmp[2] = a[0] * b[1] - a[1] * b[0];
        return tmp;
    }

    T dot(const matrix<T,R,C>& b) const {
        static_assert( C == 1, "This method is only for vectors");
        float r = 0.0;
        for (int i = 0; i < R; ++i) r += data[i] * b[i];
        return r;
    }
    matrix<T,R,C>& hdivide() {
        float w = *(data.end() - 1);
        transform(data, [&](T x) { return x / w; });
        return *this;
    }
};

template <class T, int R, int C> 
inline matrix<T,R,C> operator+(const matrix<T,R,C>& a, const matrix<T,R,C>& b) {
    matrix<T,R,C> r = a;
    r += b;
    return r;
}

template <class T, int R, int C> 
inline matrix<T,R,C> operator-(const matrix<T,R,C>& a, const matrix<T,R,C>& b) {
    matrix<T,R,C> r = a;
    r -= b;
    return r;
}

template <class T, int R, int C> 
inline matrix<T,R,C> operator*(const matrix<T,R,C>& a, const T& b) {
    matrix<T,R,C> r = a;
    r *= b;
    return r;
}

template <class T, int R, int C> 
inline matrix<T,R,C> operator*(const T& a, const matrix<T,R,C>& b) {
    matrix<T,R,C> r = b;
    r *= a;
    return r;
}

template <class T, int R, int C> 
inline matrix<T,R,C> operator/(const matrix<T,R,C>& a, const T& b) {
    matrix<T,R,C> r = a;
    r /= b;
    return r;
}

template <class T, int R1, int C1, int R2, int C2>
matrix<T, R1, C2> operator*(const matrix<T, R1, C1>& a, const matrix<T, R2, C2>& b) {
    matrix<T, R1, C2> r;
    static_assert( C1 == R2, "Invalid matrix multiplication");
    for (int i = 0; i < R1; ++i) {
        for (int j = 0; j < C2; ++j) {
            for (int n = 0; n < C1; ++n) {
                r(i,j) += a(i, n) * b(n, j);
            }
        }
    }
    return r;
}

typedef matrix<float,2,1> vec2f;
typedef matrix<float,3,1> vec3f;
typedef matrix<float,4,1> vec4f;
typedef matrix<float,4,4> matrix4f;

typedef matrix<float,3,1> vertex;
typedef matrix<float,3,1> normal;
